
from __future__ import print_function


try:
    sys.path.append(glob.glob('./carla/dist/carla-*%d.%d-%s.egg' % (
        sys.version_info.major,
        sys.version_info.minor,
        'win-amd64' if os.name == 'nt' else 'linux-x86_64'))[0])
except IndexError:
    pass

# ==============================================================================
# -- find carla module ---------------------------------------------------------
# ==============================================================================
import collections
import glob
import math
import os
import sys
import time
import weakref
import pyproj
import carla
from std_msgs.msg import String

from datetime import datetime, timedelta
import rospy
import pynmea2

# Connect to client
client = carla.Client('127.0.0.1', 2000)
client.set_timeout(2.0)

# Get World and Actors
world = client.get_world()
current_map = world.get_map()
actors = world.get_actors()

#获取所有车辆的位置信息
def get_all_location():
    location=[]
    for actor in actors:

        #遍历每一辆车：if 'vehicle' in actor.type_id:

        #遍历每一辆远车
        if 'vehicle.audi.tt' == actor.type_id:

            #t = actor.get_transform()
            #v = actor.get_velocity()
            #c = actor.get_control()

            #print(actor.type_id)
            location.append([t.location.x,t.location.y,t.location.z])

    return location

#获取本车actor（全城都开奥迪，只本车开特斯拉）
def get_my_car():
    for actor in actors:
        if actor.type_id == 'vehicle.tesla.model3':
            return actor
    return 0

#获取车辆速度
def get_speed(actor):
    v = actor.get_velocity()
    speed = int(3.6 * math.sqrt(v.x**2 + v.y**2 + v.z**2))  # km/h
    return speed

#获取目标车的gps坐标
def get_gps(actor):
    t = actor.get_transform()
    return xy2gps(-t.location.y,-t.location.x,t.location.z)

# 将xy坐标转换为gps坐标
geod = pyproj.Geod(ellps = "WGS84")     
def xy2gps(x,y,elevation):
    dis = math.sqrt(math.pow(x,2)+math.pow(y,2))
    azm = math.degrees(math.atan2(-y,x))
    #设置（125.1550768,43.8334643）为初始点坐标
    lon, lat, re_back_azm = geod.fwd(125.1550768,43.8334643,azm,dis)
    lon = NMEAConvertDegreeMinutes(lon)
    lat = NMEAConvertDegreeMinutes(lat)
    return lon, lat, elevation
 
 
def NMEAConvertDegreeMinutes(angleDec):
    if(angleDec >= 0):
        sign = 1.0 
    else:
        sign = -1.0;  
    deg = float(int(abs(angleDec)))
    return sign * (deg * 100 + (abs(angleDec) - deg) * 60.0 )

# double NMEAConvertDegreeMinutes(double angleDec) 
# { double sign;
#   double deg;
#   sign = angleDec >= 0 ? 1.0 : -1.0;  
#   deg = (double)(int)abs(angleDec);  
#   return sign * (deg*100.0 + (abs(angleDec) - deg) * 60.0); }
def getGPS(car):
    t = car.get_transform()
    lon, lat, elevation=get_gps(car)
    speed = get_speed(car)/1.852
    speed=round(speed,2) #单位：1节=1.852km/h
    utc_time = (datetime.now() - timedelta(hours=8)).strftime("%H%M%S.%f")
    utc_date = (datetime.now() - timedelta(hours=8)).strftime("%y%m%d")
    
    
    yaw = t.rotation.yaw 
    if(yaw < -90):
        yaw = 450 + yaw
    else:
        yaw = 90 + yaw
    print(yaw)

    gprmc = pynmea2.RMC('GP','RMC', (
        str(utc_time)[:-3], 'A', str(lat)[:10], 'N', str(lon)[:11], 'E', str(speed), str(yaw)[:4], utc_date, '', '', 'A'
        #str(utc_time)[:-3], 'A', "4350.56245" , 'N', "12509.40254", 'E', str(speed), str(yaw)[:4], utc_date, '', '', 'A'
        #43°50'8.2482"  	125°9'14.491439999999999"
    ))

    gpgga = pynmea2.GGA('GP','GGA',(
        str(utc_time)[:-3], str(lat)[:10], 'N', str(lon)[:11], 'E', '1', '12', str(elevation)[:4], '0.5', 'M', '', '', '','0000'
       #str(utc_time)[:-3], "4350.56245", 'N', "12509.40254", 'E', '1', '12', str(elevation)[:4], '0.5', 'M', '', '', '','0000'
    ))

    return gprmc,gpgga

def talker(car):
    pub = rospy.Publisher('gps', String, queue_size=10)
    rospy.init_node('telsa_gps', anonymous=True)
    rate = rospy.Rate(10) # 1hz
    i=0
    while not rospy.is_shutdown():
        GPRMC,GPGGA = getGPS(car)
        #这里test就像构造函数般使用，若有多个msg，那么写成test(a,b,c,d...)
        if(i%2 == 0):
            gps = GPRMC
        else:
            gps = GPGGA
        i = i+1
        #rospy.loginfo(gps)
        pub.publish(str(gps))
        rate.sleep()

if __name__ == '__main__':
    
    car=get_my_car()
    #lon,lat,back_azm=get_gps(car)
    talker(car)


    
